
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN"
  "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">

<html xmlns="http://www.w3.org/1999/xhtml">
  <head>
    <meta http-equiv="X-UA-Compatible" content="IE=Edge" />
    <meta http-equiv="Content-Type" content="text/html; charset=utf-8" />
    <title>pypcd.sautil &#8212; pypcd 0.1.1 documentation</title>
    <link rel="stylesheet" href="../../_static/alabaster.css" type="text/css" />
    <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
    <script type="text/javascript" src="../../_static/documentation_options.js"></script>
    <script type="text/javascript" src="../../_static/jquery.js"></script>
    <script type="text/javascript" src="../../_static/underscore.js"></script>
    <script type="text/javascript" src="../../_static/doctools.js"></script>
    <link rel="index" title="Index" href="../../genindex.html" />
    <link rel="search" title="Search" href="../../search.html" />
   
  <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
  
  
  <meta name="viewport" content="width=device-width, initial-scale=0.9, maximum-scale=0.9" />

  </head><body>
  

    <div class="document">
      <div class="documentwrapper">
        <div class="bodywrapper">
          

          <div class="body" role="main">
            
  <h1>Source code for pypcd.sautil</h1><div class="highlight"><pre>
<span></span><span class="sd">&quot;&quot;&quot; Utilities to operate on structured array files directly,</span>
<span class="sd">such as the ``pc_data`` in ``PointCloud`` objects.</span>
<span class="sd">&quot;&quot;&quot;</span>


<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>


<div class="viewcode-block" id="transform_xyz"><a class="viewcode-back" href="../../pypcd.html#pypcd.sautil.transform_xyz">[docs]</a><span class="k">def</span> <span class="nf">transform_xyz</span><span class="p">(</span><span class="n">T_a_b</span><span class="p">,</span> <span class="n">xyz</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; Transforms an Nx3 array xyz in frame a to frame b</span>
<span class="sd">    T_a_b is a 4x4 matrix s.t. xyz_b = T_a_b * xyz_a</span>
<span class="sd">    conversely, T_a_b is the pose a in the frame b</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="c1"># xyz in frame a, homogeneous</span>
    <span class="n">xyz1_a</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">xyz</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ones</span><span class="p">((</span><span class="mi">1</span><span class="p">,</span> <span class="n">xyz</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]))])</span>
    <span class="c1"># xyz in b frame</span>
    <span class="n">xyz1_b</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">T_a_b</span><span class="p">,</span> <span class="n">xyz1_a</span><span class="p">)</span>
    <span class="n">xyz_b</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">ascontiguousarray</span><span class="p">((</span><span class="n">xyz1_b</span><span class="p">[:</span><span class="mi">3</span><span class="p">])</span><span class="o">.</span><span class="n">T</span><span class="p">)</span></div>
    <span class="k">return</span> <span class="n">xyz_b</span>


<div class="viewcode-block" id="transform_cloud_array"><a class="viewcode-back" href="../../pypcd.html#pypcd.sautil.transform_cloud_array">[docs]</a><span class="k">def</span> <span class="nf">transform_cloud_array</span><span class="p">(</span><span class="n">T_a_b</span><span class="p">,</span> <span class="n">pc_data</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; transforms structured array. looks for xyz and xyz_origin &quot;&quot;&quot;</span>
    <span class="n">xyz</span> <span class="o">=</span> <span class="n">get_xyz_array</span><span class="p">(</span><span class="n">pc_data</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">pc_data</span><span class="o">.</span><span class="n">dtype</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
    <span class="n">xyz_b</span> <span class="o">=</span> <span class="n">transform_xyz</span><span class="p">(</span><span class="n">T_a_b</span><span class="p">,</span> <span class="n">xyz</span><span class="p">)</span>
    <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;x&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="n">xyz_b</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span>
    <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="n">xyz_b</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span>
    <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="n">xyz_b</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">]</span>
    <span class="k">if</span> <span class="s1">&#39;x_origin&#39;</span> <span class="ow">in</span> <span class="n">pc_data</span><span class="p">:</span>
        <span class="n">xyz_origin</span> <span class="o">=</span> <span class="n">get_xyz_viewpoint_array</span><span class="p">(</span><span class="n">pc_data</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">pc_data</span><span class="o">.</span><span class="n">dtype</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
        <span class="n">xyz_origin_b</span> <span class="o">=</span> <span class="n">transform_xyz</span><span class="p">(</span><span class="n">T_a_b</span><span class="p">,</span> <span class="n">xyz_origin</span><span class="p">)</span>
        <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;x_origin&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="n">xyz_origin_b</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span>
        <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y_origin&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="n">xyz_origin_b</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span>
        <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z_origin&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="n">xyz_origin_b</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">]</span></div>
    <span class="k">return</span> <span class="n">pc_data</span>


<div class="viewcode-block" id="flip_around_x"><a class="viewcode-back" href="../../pypcd.html#pypcd.sautil.flip_around_x">[docs]</a><span class="k">def</span> <span class="nf">flip_around_x</span><span class="p">(</span><span class="n">pc_data</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; flip a structured array around x, in place&quot;&quot;&quot;</span>
    <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y&#39;</span><span class="p">]</span>
    <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z&#39;</span><span class="p">]</span>

    <span class="k">if</span> <span class="s1">&#39;x_origin&#39;</span> <span class="ow">in</span> <span class="n">pc_data</span><span class="p">:</span>
        <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y_origin&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y_origin&#39;</span><span class="p">]</span></div>
        <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z_origin&#39;</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z_origin&#39;</span><span class="p">]</span>


<div class="viewcode-block" id="get_xyz_array"><a class="viewcode-back" href="../../pypcd.html#pypcd.sautil.get_xyz_array">[docs]</a><span class="k">def</span> <span class="nf">get_xyz_array</span><span class="p">(</span><span class="n">pc_data</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; get Nx3 array from structured array &quot;&quot;&quot;</span>
    <span class="k">if</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">ndim</span> <span class="o">==</span> <span class="mi">2</span> <span class="ow">and</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
        <span class="n">pc_data</span> <span class="o">=</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">squeeze</span><span class="p">()</span>
    <span class="n">xyz</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="nb">len</span><span class="p">(</span><span class="n">pc_data</span><span class="p">),</span> <span class="mi">3</span><span class="p">),</span> <span class="n">dtype</span><span class="o">=</span><span class="n">dtype</span><span class="p">)</span>
    <span class="n">xyz</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;x&#39;</span><span class="p">]</span>
    <span class="n">xyz</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y&#39;</span><span class="p">]</span>
    <span class="n">xyz</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z&#39;</span><span class="p">]</span></div>
    <span class="k">return</span> <span class="n">xyz</span>


<div class="viewcode-block" id="get_xyz_viewpoint_array"><a class="viewcode-back" href="../../pypcd.html#pypcd.sautil.get_xyz_viewpoint_array">[docs]</a><span class="k">def</span> <span class="nf">get_xyz_viewpoint_array</span><span class="p">(</span><span class="n">pc_data</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot; get Nx3 array from structured array &quot;&quot;&quot;</span>
    <span class="k">if</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">ndim</span> <span class="o">==</span> <span class="mi">2</span> <span class="ow">and</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
        <span class="n">pc_data</span> <span class="o">=</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">squeeze</span><span class="p">()</span>
    <span class="n">xyz</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="nb">len</span><span class="p">(</span><span class="n">pc_data</span><span class="p">),</span> <span class="mi">3</span><span class="p">),</span> <span class="n">dtype</span><span class="o">=</span><span class="n">dtype</span><span class="p">)</span>
    <span class="n">xyz</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;x_origin&#39;</span><span class="p">]</span>
    <span class="n">xyz</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y_origin&#39;</span><span class="p">]</span>
    <span class="n">xyz</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z_origin&#39;</span><span class="p">]</span></div>
    <span class="k">return</span> <span class="n">xyz</span>


<div class="viewcode-block" id="get_xyzl_array"><a class="viewcode-back" href="../../pypcd.html#pypcd.sautil.get_xyzl_array">[docs]</a><span class="k">def</span> <span class="nf">get_xyzl_array</span><span class="p">(</span><span class="n">pc_data</span><span class="p">,</span> <span class="n">dtype</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">float32</span><span class="p">):</span>
    <span class="k">if</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">ndim</span> <span class="o">==</span> <span class="mi">2</span> <span class="ow">and</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
        <span class="n">pc_data</span> <span class="o">=</span> <span class="n">pc_data</span><span class="o">.</span><span class="n">squeeze</span><span class="p">()</span>
    <span class="n">xyzl</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">empty</span><span class="p">((</span><span class="nb">len</span><span class="p">(</span><span class="n">pc_data</span><span class="p">),</span> <span class="mi">4</span><span class="p">),</span> <span class="n">dtype</span><span class="o">=</span><span class="n">dtype</span><span class="p">)</span>
    <span class="n">xyzl</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;x&#39;</span><span class="p">]</span>
    <span class="n">xyzl</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;y&#39;</span><span class="p">]</span>
    <span class="n">xyzl</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;z&#39;</span><span class="p">]</span>
    <span class="n">xyzl</span><span class="p">[:,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">pc_data</span><span class="p">[</span><span class="s1">&#39;label&#39;</span><span class="p">]</span></div>
    <span class="k">return</span> <span class="n">xyzl</span>
</pre></div>

          </div>
          
        </div>
      </div>
      <div class="sphinxsidebar" role="navigation" aria-label="main navigation">
        <div class="sphinxsidebarwrapper"><div class="relations">
<h3>Related Topics</h3>
<ul>
  <li><a href="../../index.html">Documentation overview</a><ul>
  <li><a href="../index.html">Module code</a><ul>
  </ul></li>
  </ul></li>
</ul>
</div>
<div id="searchbox" style="display: none" role="search">
  <h3>Quick search</h3>
    <div class="searchformwrapper">
    <form class="search" action="../../search.html" method="get">
      <input type="text" name="q" />
      <input type="submit" value="Go" />
      <input type="hidden" name="check_keywords" value="yes" />
      <input type="hidden" name="area" value="default" />
    </form>
    </div>
</div>
<script type="text/javascript">$('#searchbox').show(0);</script>
        </div>
      </div>
      <div class="clearer"></div>
    </div>
    <div class="footer">
      &copy;2018, Daniel Maturana.
      
      |
      Powered by <a href="http://sphinx-doc.org/">Sphinx 1.7.1</a>
      &amp; <a href="https://github.com/bitprophet/alabaster">Alabaster 0.7.12</a>
      
    </div>

    

    
  </body>
</html>